#!/usr/bin/env python3
# -*- coding: utf-8 -*-

import os
import rclpy
from rclpy.node import Node
from rclpy.qos import QoSProfile
# 正确导入所需消息类型 [[6]][[8]]
from geometry_msgs.msg import PoseStamped, Twist, PointCloud2
from nav_msgs.msg import Path, OccupancyGrid  # 新增OccupancyGrid [[1]][[6]]
from tf2_msgs.msg import TFMessage  # 正确导入TFMessage [[3]]

class BridgeNode(Node):
    def __init__(self):
        super().__init__('bridge_node')
        self.get_logger().info('BridgeNode has been started.')
        
        qos = QoSProfile(depth=10)  # 统一QoS配置 [[9]]
        
        # 导航目标桥接（ROS2→ROS1）
        self.goal_pub = self.create_publisher(
            PoseStamped,
            '/move_base_simple/goal',
            qos
        )
        
        # 订阅ROS2端需要转发的话题
        self.create_subscription(
            PoseStamped,
            '/move_base_goal',
            self.goal_callback,
            qos
        )
        self.create_subscription(
            Twist,
            '/cmd_vel',
            self.cmd_vel_callback,
            qos
        )
        self.create_subscription(
            TFMessage,
            '/tf',
            self.tf_callback,
            qos
        )
        self.create_subscription(
            PointCloud2,
            '/cloud_registered',
            self.cloud_callback,
            qos
        )
        self.create_subscription(
            Path,
            '/move_base1/DWAPlannerROS/local_plan',
            self.handle_local_plan,
            qos
        )
        self.create_subscription(
            Path,
            '/move_base1/NavfnROS/plan',
            self.handle_global_plan_callback,
            qos
        )
        # 修正地图话题类型 [[1]][[6]]
        self.create_subscription(
            OccupancyGrid,
            '/prior_map',
            self.handle_map_callback,
            qos
        )
        
    def goal_callback(self, msg):
        self.get_logger().info(f'Forwarding goal: {msg}')
        self.goal_pub.publish(msg)

    def cmd_vel_callback(self, msg):
        self.get_logger().info(f'Received cmd_vel: {msg}')

    def tf_callback(self, msg):
        self.get_logger().info(f'Received TF: {msg}')

    def cloud_callback(self, msg):
        self.get_logger().info(f'Received PointCloud: {msg}')

    def handle_local_plan(self, msg):
        self.get_logger().info(f'Received local plan: {msg}')

    def handle_global_plan_callback(self, msg):
        self.get_logger().info(f'Received global plan: {msg}')

    def handle_map_callback(self, msg):
        self.get_logger().info(f'Received map: {msg}')

def main(args=None):
    rclpy.init(args=args)
    node = BridgeNode()
    try:
        rclpy.spin(node)
    except KeyboardInterrupt:
        node.get_logger().info('Shutting down bridge node...')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()